extends Node
class_name MyAstar

#******************************************AStar算法部分******************************************
class SearchEntry:
	var s_pos:Vector2i
	var g_cost
	var f_cost
	var pre_entry
	func _init(_s_pos, _g_cost, _f_cost=0, _pre_entry=null):
		s_pos=_s_pos
		g_cost = _g_cost
		f_cost = _f_cost
		pre_entry = _pre_entry
	func getPos():
		return s_pos
		
func getNewPosition(map:Array[Vector2i], obstacles:Array[Vector2i],location:SearchEntry, offset:Vector2i):
	var _pos:Vector2i=location.s_pos+offset
	if map.has(_pos) and not obstacles.has(_pos):
		return _pos
	
func getPositions(map:Array[Vector2i],obstacles:Array[Vector2i],location:SearchEntry):
	# use four ways or eight ways to move
	var offsets:Array[Vector2i]= [Vector2i(0,1),Vector2i(1,0),Vector2i(-1,0),Vector2i(0,-1)]
	var poslist:Array[Vector2i]=[]
	for offset in offsets:
		var _pos = getNewPosition(map, obstacles,location, offset)
		if _pos!=null:			
			poslist.append(_pos)
	return poslist	
# imporve the heuristic distance more precisely in future
func calHeuristic(_pos:Vector2i, dest:SearchEntry):
	return abs(dest.s_pos.x - _pos.x) + abs(dest.s_pos.y - _pos.y)

func getMoveCost(location:SearchEntry, _pos:Vector2i):
	if location.s_pos!= _pos:
		return 1.4
	else:
		return 1

# check if the position is in list
func isInList(list:Dictionary, _pos:Vector2i):
	if _pos in list:
		return list[_pos]
	return null

# add available adjacent positions
func addAdjacentPositions(map:Array[Vector2i], obstacles:Array[Vector2i],location:SearchEntry,dest:SearchEntry, openlist:Dictionary, closedlist:Dictionary):
	var poslist:Array[Vector2i] = getPositions(map, obstacles,location)
	for _pos in poslist:
		# if position is already in closedlist, do nothing
		if isInList(closedlist, _pos)==null:
			var findEntry = isInList(openlist, _pos)
			var h_cost = calHeuristic(_pos, dest)
			var g_cost = location.g_cost + getMoveCost(location, _pos)
			if findEntry==null :
				# if position is not in openlist, add it to openlist
				openlist[_pos] = SearchEntry.new(_pos, g_cost, g_cost+h_cost, location)
			elif findEntry.g_cost > g_cost:
				# if position is in openlist and cost is larger than current one,
				# then update cost and previous position
				findEntry.g_cost = g_cost
				findEntry.f_cost = g_cost + h_cost
				findEntry.pre_entry = location
# find a least cost position in openlist, return None if openlist is empty
func getFastPosition(openlist):
	var fast = null
	for entry in openlist.values():
		if fast==null:
			fast = entry
		elif fast.f_cost > entry.f_cost:
			fast = entry
	return fast
func AStarSearch(map:Array[Vector2i], obstacles:Array[Vector2i],source:Vector2i, _dest:Vector2i):
	var openlist:Dictionary = {}
	var closedlist:Dictionary = {}
	var location:SearchEntry = SearchEntry.new(source, 0.0)
	var dest:SearchEntry = SearchEntry.new(_dest, 0.0)
	openlist[source] = location
	while true:
		location = getFastPosition(openlist)
		if location==null:
			# not found valid path
			break;
		if location.s_pos== dest.s_pos:
			break
		closedlist[location.getPos()] = location
		openlist.erase(location.getPos())
		addAdjacentPositions(map, obstacles,location, dest, openlist, closedlist)
	var move_path:Array[Vector2i]=[]
	while location!=null:
		var _pos:Vector2i=location.s_pos
		move_path.push_front(_pos)
		location = location.pre_entry		
	return move_path
